
// import oimo.collision.narrowphase.DetectorResultPoint;
// import oimo.m.Vec3;
// import oimo.m.M;
// import oimo.common.Vec3;
// import oimo.collision.narrowphase.*;
// import oimo.common.Transform;

import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { DetectorResultPoint } from "../../../collision/narrowphase/DetectorResultPoint";
import { Transform } from "../../../common/Transform";
import { ContactImpulse } from "./ContactImpulse";

/**
 * A manifold point is a contact point in a contact manifold. This holds detailed collision
 * data (position, overlap depth, impulse, etc...) for collision response.
 */
export class ManifoldPoint {
	// manifold point relative to rigid bodies. NOT SHAPES.
	_localPos1: Vec3;
	_localPos2: Vec3;

	// local position with rotation
	_relPos1: Vec3;
	_relPos2: Vec3;

	// world position
	_pos1: Vec3;
	_pos2: Vec3;
	_depth: number;

	_impulse: ContactImpulse;

	_warmStarted: boolean;

	// manifold points can be disabled for some reasons (separated, etc...)
	_disabled: boolean;

	_id: number;


	constructor() {
		this._localPos1 = new Vec3();
		this._localPos2 = new Vec3();
		this._relPos1 = new Vec3();
		this._relPos2 = new Vec3();
		this._pos1 = new Vec3();
		this._pos2 = new Vec3();
		this._depth = 0;

		this._impulse = new ContactImpulse();

		this._warmStarted = false;
		this._disabled = false;
		this._id = -1;
	}

	// --- internal ---


	public _clear(): void {
		this._localPos1.zero();
		this._localPos2.zero();
		this._relPos1.zero();
		this._relPos2.zero();
		this._pos1.zero();
		this._pos2.zero();
		this._depth = 0;
		this._impulse.clear();
		this._warmStarted = false;
		this._disabled = false;
		this._id = -1;
	}


	public _initialize(result: DetectorResultPoint, tf1: Transform, tf2: Transform): void {
		// world position
		this._pos1.copyFrom(result.position1);
		this._pos2.copyFrom(result.position2);

		// local position with rotation
		this._pos1.subTo(tf1._position, this._relPos1);
		this._pos2.subTo(tf2._position, this._relPos2);

		// local position
		this._relPos1.mulMat3to(tf1._rotation, this._localPos1);
		this._relPos2.mulMat3to(tf2._rotation, this._localPos2);

		this._depth = result.depth;

		this._impulse.clear();

		this._id = result.id;
		this._warmStarted = false;
		this._disabled = false;
	}


	public _updateDepthAndPositions(result: DetectorResultPoint, tf1: Transform, tf2: Transform): void {
		// world position
		this._pos1.copyFrom(result.position1);
		this._pos2.copyFrom(result.position2);

		// local position with rotation
		this._pos1.subTo(tf1._position, this._relPos1);
		this._pos2.subTo(tf2._position, this._relPos2);

		// local position
		this._relPos1.mulMat3to(tf1._rotation, this._localPos1);
		this._relPos2.mulMat3to(tf2._rotation, this._localPos2);

		this._depth = result.depth;
	}


	public _copyFrom(cp: ManifoldPoint): void {
		M.vec3_assign(this._localPos1, cp._localPos1);
		M.vec3_assign(this._localPos2, cp._localPos2);
		M.vec3_assign(this._relPos1, cp._relPos1);
		M.vec3_assign(this._relPos2, cp._relPos2);
		M.vec3_assign(this._pos1, cp._pos1);
		M.vec3_assign(this._pos2, cp._pos2);
		this._depth = cp._depth;
		this._impulse.copyFrom(cp._impulse);
		this._id = cp._id;
		this._warmStarted = cp._warmStarted;
		this._disabled = false;
	}

	// --- public ---

	/**
	 * Returns the first rigid body's manifold point in world coordinate.
	 */
	public getPosition1(): Vec3 {
		return this._pos1.clone();
	}

	/**
	 * Sets `position` to the first rigid body's manifold point in world coordinate.
	 * This does not create a new instance of `Vec3`.
	 */
	public getPosition1To(position: Vec3): void {
		position.copyFrom(this._pos1);
	}

	/**
	 * Returns the second rigid body's manifold point in world coordinate.
	 */
	public getPosition2(): Vec3 {
		return this._pos2.clone();
	}

	/**
	 * Sets `position` to the second rigid body's manifold point in world coordinate.
	 * This does not create a new instance of `Vec3`.
	 */
	public getPosition2To(position: Vec3): void {
		position.copyFrom(this._pos2);
	}

	/**
	 * Returns the amount of the overlap. If the manifold point is separate, a negative
	 * value is returned.
	 */
	public getDepth(): number {
		return this._depth;
	}

	/**
	 * Returns whether the manifold point has existed for more than two steps.
	 */
	public isWarmStarted(): boolean {
		return this._warmStarted;
	}

	/**
	 * Returns the normal impulse of the manifold point.
	 */
	public getNormalImpulse(): number {
		return this._impulse.impulseN;
	}

	/**
	 * Returns the tangent impulse of the manifold point.
	 */
	public getTangentImpulse(): number {
		return this._impulse.impulseT;
	}

	/**
	 * Returns the binormal impulse of the manifold point.
	 */
	public getBinormalImpulse(): number {
		return this._impulse.impulseB;
	}

	/**
	 * Returns whether the manifold point is enabled.
	 */
	public isEnabled(): boolean {
		return !this._disabled;
	}

}
